/*
 * Droplet.cpp
 *
 * Created: 5/11/2012 4:13:57 PM
 *  Author: common
 */ 

#include <avr/io.h>
#include <math.h>
#include <util/delay.h>

#include "motor.h"
#include "RGB_LED.h"
#include "pc_com.h"
#include "RGB_sensor.h"
#include "power.h"

int main(void)
{
	motor_init();
	RGB_LED_init();
	pc_com_init();
	
	leg_monitor_init();
	
	int8_t l1, l2, l3;
	char i = 0;
	
	printf("Press any key to pause/unpause. \r\n");
	while (1)
	{
		if (PC_USART.STATUS & USART_RXCIF_bm)
		{
			i = PC_USART.DATA;
			printf("\r\nPaused (%c) \r\n", i);
			while (!(PC_USART.STATUS & USART_RXCIF_bm));	
			i = PC_USART.DATA;
		}
	
		printf("%c  ", ++i % 26 + 'A');
		
		switch (leg1_status())
		{
			case (1): printf("LEG 1 IS POWERED \t"); set_red_led(100); break;
			case (-1): printf("LEG 1 IS GROUNDED \t"); set_red_led(25); break;
			case (0): printf("LEG 1 IS FLOATING \t"); set_red_led(0); break;
			default: printf("LEG 1 ERROR \t");
		}
		
		switch (leg2_status())
		{
			case (1): printf("LEG 2 IS POWERED \t"); set_green_led(100); break;
			case (-1): printf("LEG 2 IS GROUNDED \t"); set_green_led(25); break;
			case (0): printf("LEG 2 IS FLOATING \t"); set_green_led(0); break;
			default: printf("LEG 2 ERROR \t");
		}
		
		switch (leg3_status())
		{
			case (1): printf("LEG 3 IS POWERED \r\n"); set_blue_led(100); break;
			case (-1): printf("LEG 3 IS GROUNDED \r\n"); set_blue_led(25); break;
			case (0): printf("LEG 3 IS FLOATING \r\n"); set_blue_led(0); break;
			default: printf("LEG 3 ERROR \r\n");
		}
		_delay_ms(500);
	}	
}